#include "motionLoader.h"

MotionLoader::MotionLoader() {
}

MotionLoader::~MotionLoader() {
}

Motion* MotionLoader::load(string filename) {
   FileType id = noneFile;
   if (filename.substr(filename.size() - 4, 4) == ".amc") {
      id = amcFile;
   } else if (filename.substr(filename.size() - 4, 4) == ".bvh") {
      id = bvhFile;
   }
   if (id == noneFile) {
      cout << "File format not recognizeg!" << endl;
      return 0;
   } else {
      return load(filename, id);
   }
}

Motion* MotionLoader::load(string filename, FileType id) {
   switch (id) {
      case amcFile:
         return loadAMC(filename);
         break;
      case bvhFile:
         return loadBVH(filename);
         break;
      default:
         break;
   }
   return 0;
}

Motion* MotionLoader::loadAMC(string filename) {
   
   Motion *m = new Motion();
   PoseTransformations pt;

   ifstream file(filename.c_str());

   string line;
   int current_frame;
   vector<string> words;
   
   words.clear();   

   if (file.is_open()) {
      file >> noskipws;
      while (!file.eof()) {
         getline(file, line);
         words = readWords(line);
         if (line[0] == ':' || line[0] == '#') { // comentary line or initial line
            if (words[0] == ":DEGREES"){
               m->setDegree(true);
            }
            continue;
         } else if (line == "" || words.size() == 1) { // initial frame line
            if (line != "")
               current_frame = (int) strtod(words[0].c_str(), NULL);
            else
               current_frame++;
            if (current_frame > 1) {
               m->insertFrame(pt);
               pt.clear();
            }
         } else {
            string current_bone = words[0];
            Transformations t;
            for (int i = 1; i < words.size(); i++){
               t.push_back(strtod(words[i].c_str(), NULL));
            }
            pt[current_bone] = t;
         }
      }
      file.close();
   } else {
      cout << "Problem on open file" << endl;
      return 0;
   }
   return m;
}

Motion* MotionLoader::loadBVH(string filename) {
   
   Motion *m = new Motion();
   PoseTransformations pt;
   
   ifstream file(filename.c_str());

   string line;
   vector<string> words;
   list< pair<string, int> > boneOrder;
   pair<string, int> current_bone;
   bool readingMotion = false;

   words.clear();
   boneOrder.clear();

   if (file.is_open()) {
      file >> noskipws;

      getline(file, line);
      words = readWords(line);
      
      if (words[0] != "HIERARCHY") {
         loadBvhFail();
         return 0;
      }

      while (!file.eof()) {
         
         getline(file, line);
         words = readWords(line);
         
         if (words[0] == "ROOT") {
            
            current_bone.first = "root";
            m->setDegree(true);

            getline(file, line);
            words = readWords(line);
            if (words[0] != "{") {
               loadBvhFail();
               return 0;
            }            

            getline(file, line);
            words = readWords(line);
            if (words[0] != "OFFSET") {
               loadBvhFail();
               return 0;
            }

            getline(file, line);
            words = readWords(line);
            if (words[0] != "CHANNELS") {
               loadBvhFail();
               return 0;
            }
            
            current_bone.second = (int) strtod(words[1].c_str(), NULL);
            boneOrder.push_back(current_bone);
            
         } else if (words[0] == "JOINT") {
            
            current_bone.first = words[1];
            
         } else if (words[0] == "End") {
            
            string ant_bone = current_bone.first;
            current_bone.first = words[0] + ant_bone;
            current_bone.second = 0;
            boneOrder.push_back(current_bone);
            
         } else if (words[0] == "CHANNELS") {
            
            current_bone.second = (int) strtod(words[1].c_str(), NULL);            
            boneOrder.push_back(current_bone);
            
         } else if (words[0] == "OFFSET" || words[0] == "{" || words[0] == "}") {
            continue;
         } else if (words[0] == "MOTION" || readingMotion) {
            
            if(!readingMotion){
               getline(file, line);
               words = readWords(line);
               if (words[0] != "Frames:") {
                  loadBvhFail();
                  return 0;
               }
   
               getline(file, line);
               words = readWords(line);
               if (words[0] + words[1] != "FrameTime:") {
                  loadBvhFail();
                  return 0;
               }
               
               getline(file, line);
               words = readWords(line);
            }
            
            readingMotion = true;
            
            list< pair< string, int> >boneOrderAux = boneOrder;
            Transformations t;
            int i = 0;
            
            while (boneOrderAux.size() > 0) {
               pair< string, int > current_bone = boneOrderAux.front();
               boneOrderAux.pop_front();
               while (current_bone.second > 0) {
                  t.push_back(strtod(words[i].c_str(), NULL));
                  i++;
                  current_bone.second--;
               }
               pt[current_bone.first] = t;
               t.clear();
            }
            
            m->insertFrame(pt);
            pt.clear();
         }
      }      
   } else {
      cout << "Problem on open file" << endl;
      return 0;
   }
   
   return m;
}
